#ifndef __CR_MATH_MATRIX_H
#define __CR_MATH_MATRIX_H

#include "HSL/HSL-pch.h"
#include "crMath.h"

namespace HSL {

template <int rows, int cols = rows, class T = double>
struct Matrix
{
	static const Matrix null;
	T p[rows][cols];
	Matrix<cols, rows, T> trans() const
	{
		Matrix<cols, rows, T> t;
		for(int i=0;i<rows;i++)
			for(int j=0;j<cols;j++)
				t.p[j][i]=p[i][j];
		return t;
	}
	T det(T epsilon = 0) const
	{
		if( rows != cols || rows <= 0 )
			return 0;
		if(rows < 4)
		{
			switch(rows)
			{
				case 1: //1x1
					return
						 p[0][0];
				case 2: //2x2
					return
						+p[0][0]*p[1][1]
						-p[0][1]*p[1][0];
				case 3: //3x3
					return 
						 p[0][0]*p[1][1]*p[2][2]
    					+p[0][1]*p[1][2]*p[2][0]
    					+p[1][0]*p[2][1]*p[0][2]
    					-p[2][0]*p[1][1]*p[0][2]
    					-p[2][1]*p[1][2]*p[0][0]
    					-p[1][0]*p[0][1]*p[2][2];
			}
		}
		Matrix tmpm = *this;
		bool degen = false;
		register int k = 0, col, row;
		T a, d = p[0][0], (*tmp)[cols] = tmpm.p;
		while(k<rows-1)
		{
			if(abs(tmp[k][k])<=epsilon)
			{
				degen=true;
				for (row=k+1;row<rows;row++)
				{
					if(abs(tmp[row][k]) > epsilon)
					{
						degen=false;
						// Add Row 'row' To Row 'k'
						for(col=0;col<rows;col++)
							tmp[k][col]+=tmp[row][col];
						break;
					}
				}
				if(degen)
					return 0;
			}
			for (row=k+1;row<rows;row++)
			{
				a=-tmp[row][k]/tmp[k][k];
				//Add Row 'k' Multiplied By 'a' To Row 'row'
				for(col=k;col<rows;col++)
					tmp[row][col]+=a*tmp[k][col];
			}
			k++;
			d*=tmp[k][k];
		}
		return d;
	}
	Matrix<rows, rows, T> inv(T epsilon = 0) const
	{
		if( rows != cols || rows <= 0 )
			return null;
		if(rows < 4)
		{
			T d = det(epsilon);
			if( abs(d) <= epsilon )
				return null;
			T den = 1/d;
			switch(rows)
			{
				case 1: // 1x1
				{
					Matrix<rows, rows, T> out = {den};
					return out;
				}
				case 2: // 2x2
				{
					Matrix<rows, rows, T> out = {
						 p[1][1]*den, -p[0][1]*den,
						-p[1][0]*den,  p[0][0]*den
					};
					return out;
				}
				case 3: // 3x3
				{
					Matrix<rows, rows, T> out = {
						(p[2][2]*p[1][1]-p[2][1]*p[1][2])*den, (p[2][1]*p[0][2]-p[2][2]*p[0][1])*den, (p[1][2]*p[0][1]-p[1][1]*p[0][2])*den,
						(p[2][0]*p[1][2]-p[2][2]*p[1][0])*den, (p[2][2]*p[0][0]-p[2][0]*p[0][2])*den, (p[1][0]*p[0][2]-p[1][2]*p[0][0])*den,
						(p[2][1]*p[1][0]-p[2][0]*p[1][1])*den, (p[2][0]*p[0][1]-p[2][1]*p[0][0])*den, (p[1][1]*p[0][0]-p[1][0]*p[0][1])*den
					};
					return out;
				}
			}
		}
		Matrix<rows, rows, T> outm;
		Matrix<rows, cols, T> tmpm = *this;
		outm.one();
		T a;
		register int i,j,k;
		bool degen=false;
		for (i = 0; i<rows-1; i++)
		{
			if(abs(tmpm.p[i][i])<=epsilon)
			{
				degen=true;
				for (k = i+1; k<rows; k++)
				{
					if(abs(tmpm.p[k][i])>epsilon)
					{
						degen=false;
						for (j = 0; j<rows; j++)
						{
							tmpm.p[i][j]+=tmpm.p[k][j];
							outm.p[i][j]+=outm.p[k][j];
						}
						break;
					}
				}
				if(degen)
					return Matrix<rows, rows>::null;
			}
			a=1/tmpm.p[i][i];
			for (j = 0; j<rows; j++)
			{
				tmpm.p[i][j]*=a;
				outm.p[i][j]*=a;
			}
			for (k = i+1; k<rows; k++)
			{
				a=-tmpm.p[k][i];
				for (j = 0; j<rows; j++)
				{
					tmpm.p[k][j]+=a*tmpm.p[i][j];
					outm.p[k][j]+=a*outm.p[i][j];
				}
			}
		}
		if(abs(tmpm.p[rows-1][rows-1])<=epsilon)
			return null;
		a= 1/tmpm.p[rows-1][rows-1];
		for (j = 0; j<rows; j++)
		{
			tmpm.p[rows-1][j]*=a;
			outm.p[rows-1][j]*=a;
		}
		for (i = rows-1; i>0; i--)
		{
			for (k = i-1; k>=0; k--)
			{
				a=-tmpm.p[k][i];
				for (j = 0; j<rows; j++)
					outm.p[k][j]+=a*outm.p[i][j];
			}
		}
		return outm;
	}
	Matrix<cols, rows, T> pseudoInv(T epsilon = 0.0) const
	{
		Matrix<cols, rows, T> mt = this->trans();
		return (mt * (*this)).inv() * mt;
	}
	T trace() const
	{
		T result = 0;
		for(int i=0;i<rows&&i<cols;i++)
			result+=p[i][i];
		return result;    
	}
	T* operator[] (int i) { return p[i]; }
	const T* operator[] (int i) const { return p[i]; }
	bool operator == (const Matrix<rows, cols, T> &m) const
	{
		return memcmp(p, m.p, sizeof(*p)) == 0;
	}
	bool operator != (const Matrix<rows, cols, T> &m) const
	{
		return memcmp(p, m.p, sizeof(*p)) != 0;
	}
	void clear()
	{
		memset(p, 0, sizeof(*p));
	}
	void one()
	{
		for(int i=0;i<rows;i++)
			for(int j=0;j<cols;j++)
				p[i][j]=i==j;
	}
};

template <int rows, int cols, class T>
const Matrix<rows, cols, T> Matrix<rows, cols, T>::null = {0};

template <int rows1, int cols1_rows2, int cols2, class T>
inline Matrix<rows1, cols2, T> operator * (const Matrix<rows1, cols1_rows2, T> &m1, const Matrix<cols1_rows2, cols2, T> &m2)
{
	Matrix<rows1, cols2, T> m = {};
	for(int i=0;i<rows1;i++)
	{
		for(int j=0;j<cols2;j++)
		{
			T tmp=0;
			for(int k=0;k<cols1_rows2;k++)
				tmp+=m1.p[i][k]*m2.p[k][j];
			m.p[i][j]=tmp;
		}
	}
	return m;
}

template <int rows, int cols, class T, class L>
inline Matrix<rows, cols, T> operator * (const Matrix<rows, cols, T> &m, L n)
{
	Matrix<rows, cols, T> out;
	for(int i=0;i<rows;i++)
		for(int j=0;j<cols;j++)
			out.p[i][j] = (T)(m.p[i][j] * n);
	return out;
}

template <int rows, int cols, class T, class L>
inline Matrix<rows, cols, T> operator * (L n, const Matrix<rows, cols, T> &m)
{
	Matrix<rows, cols, T> out;
	for(int i=0;i<rows;i++)
		for(int j=0;j<cols;j++)
			out.p[i][j] = (T)(m.p[i][j] * n);
	return out;
}

template <int rows, int cols, class T>
inline Matrix<rows, cols, T> operator + (const Matrix<rows, cols, T> &m1, const Matrix<rows, cols, T> &m2)
{
	Matrix<rows, cols, T> out;
	for(int i=0;i<rows;i++)
		for(int j=0;j<cols;j++)
			out.p[i][j] = m1.p[i][j] + m2.p[i][j];
	return out;
}

template <int rows, int cols, class T>
inline Matrix<rows, cols, T> operator - (const Matrix<rows, cols, T> &m1, const Matrix<rows, cols, T> &m2)
{
	Matrix<rows, cols, T> out;
	for(int i=0;i<rows;i++)
		for(int j=0;j<cols;j++)
			out.p[i][j] = m1.p[i][j] - m2.p[i][j];
	return out;
}

}; // namespace HSL

#endif // __CR_MATH_MATRIX_H